package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.DriverStation;

public class LineSensor extends SimpleRobot {

    DigitalInput left;
    DigitalInput center;
    DigitalInput right;
    RobotDrive drive;
    Timer timer = new Timer();
    Timer temptimer = new Timer();
    DriverStation ds;

    public LineSensor(int leftchan, int centerchan, int rightchan, RobotDrive drive){
        left = new DigitalInput(leftchan);
        center = new DigitalInput(centerchan);
        right = new DigitalInput(rightchan);
        this.drive = drive;
    }

    public void run(){
        //initialize variables
        timer.start();
        timer.reset();
        double direction;
        direction = 1;

        while (isAutonomous() && isEnabled()){
        //while(timer.get() < 60){  //runs for autonomous mode for 60 sec
            if((left.get() == false) && (center.get() == true) && (right.get() == false)){ //robot is on center
                drive.mecanumDrive_Cartesian(0, -0.25*direction, 0, 0);  //go straight at 25% speed
            }
            else if((left.get() == false) && (center.get() == false) && (right.get() == true)){ //left of center
                drive.mecanumDrive_Cartesian(0.25, -0.25*direction, 0, 0);  //go right at 25% speed to correct
            }
            else if((left.get() == true) && (center.get() == false) && (right.get() == false)){ //right of center
                drive.mecanumDrive_Cartesian(-0.25, -0.25*direction, 0, 0); //go left at 25% speed to correct
            }
            else if((left.get() == true) && (center.get() == true) && (right.get() == false)){ //twisted to the right
                drive.mecanumDrive_Cartesian(0, 0, 0.25, 0);  //twist CW at 25% speed to correct
            }
            else if((left.get() == false) && (center.get() == true) && (right.get() == true)){ //twised to the left
                drive.mecanumDrive_Cartesian(0, 0, -0.25, 0);  //twist CCW at 25% speed to correct
            }
            else if((left.get() == true) && (center.get() == true) && (right.get() == true)){ //robot is at the T
                direction = (-1) * direction; //change the direction of straight
                temptimer.start();
                temptimer.reset();
                while (temptimer.get() < 1){ //give the robot 1 second to switch directions before continuing loop.
                    drive.mecanumDrive_Cartesian(0, -0.25*direction, 0, 0); //drive in reverse to the T at the other end
                }
            }
            //CASE: (true, false, true) occurs if it reaches a fork, tell robot which way to go using the driver station
            /*
            else if((left.get() == true) && (center.get() == false) && (right.get() == true)){ //robot is at the Y fork
                if (ds.getDigitalIn(1) == true){ //take the left line
                    while(center.get() == false){ //drive left until the center finds the left line
                        drive.mecanumDrive_Cartesian(-0.25, 0, 0, 0);
                    }
                }
                else if(ds.getDigitalIn(2) == true)
                { //take the right line
                    while(center.get() == false){ //drive right until the center finds the left line
                        drive.mecanumDrive_Cartesian(0.25, 0, 0, 0);
                    }
                }
            }
            */
            else{ //(false, false, false) robot is off the line
                drive.mecanumDrive_Cartesian(0, 0, 0, 0); //stop
            }
        }
    }
}
